#include <cstring>
#include <Windows.h>
#include "voxelData.h"

#define CERTAINTY_TRESHOLD_MAX 245
#define CERTAINTY_TRESHOLD_MIN 10
#define CERTAINTY_SPEED_POSITIVE 3
#define CERTAINTY_SPEED_NEGATIVE 1

unsigned char * VoxelRegion = 0;
int VoxelSize;
int CubeSize;
int VoxelsPerRow;
int CenterVoxelPosition;

bool voxelData_InitVoxelRegion(int cubeSizeInMm, int voxelSizeInMm) {
	if (cubeSizeInMm < 1 || voxelSizeInMm < 1 || (double)cubeSizeInMm / (double)voxelSizeInMm < 1.0) {
		return false;
	}
	VoxelsPerRow = cubeSizeInMm / voxelSizeInMm;
	CubeSize = voxelSizeInMm * VoxelsPerRow;
	VoxelSize = voxelSizeInMm;
	CenterVoxelPosition = VoxelsPerRow / 2;
	if (VoxelRegion) {
		voxelData_DiscardVoxelRegion();
	}
	VoxelRegion = (unsigned char *)malloc(VoxelsPerRow * VoxelsPerRow * VoxelsPerRow);
	if (!VoxelRegion) {
		return false;
	}
	voxelData_ClearVoxelRegion();
	return true;
}

void voxelData_DiscardVoxelRegion() {
	free(VoxelRegion);
}

bool voxelData_SaveToFile(const char * fileName) {
	FILE * f = fopen(fileName, "wb");
	bool ret = false;
	if (f) {
		if (fwrite(&VoxelSize, sizeof(int), 1, f) == 1 &&
			fwrite(&CubeSize, sizeof(int), 1, f) == 1 &&
			fwrite(&VoxelsPerRow, sizeof(int), 1, f) == 1 &&
			fwrite(&CenterVoxelPosition, sizeof(int), 1, f)) 
		{
			ret = (fwrite(VoxelRegion, sizeof(char) * VoxelsPerRow * VoxelsPerRow * VoxelsPerRow, 1, f) == 1);
		}
		fclose(f);
	}
	return ret;
}

bool voxelData_LoadFromFile(const char * fileName) {
	FILE * f = fopen(fileName, "rb");
	bool ret = false;
	if (f) {
		if (fread(&VoxelSize, sizeof(int), 1, f) == 1 &&
			fread(&CubeSize, sizeof(int), 1, f) == 1 &&
			fread(&VoxelsPerRow, sizeof(int), 1, f) == 1 &&
			fread(&CenterVoxelPosition, sizeof(int), 1, f)) 
		{
			ret = voxelData_InitVoxelRegion(CubeSize, VoxelSize);
			if (ret) {
				for (int i = 0; i < VoxelsPerRow * VoxelsPerRow * VoxelsPerRow && ret; i++) {
					ret = ret && (fread(&VoxelRegion[i], sizeof(unsigned char), 1, f) == 1);
				}
			}
		}
		fclose(f);
	}
	return ret;
}

void voxelData_ClearVoxelRegion() {
	if (VoxelRegion) {
		memset(VoxelRegion, 0, VoxelsPerRow * VoxelsPerRow * VoxelsPerRow);
	}
}

int voxelData_mmToVoxel(int mm) {
	int ret = (CubeSize / 2 + mm) / VoxelSize;
	return ret;
}

int voxelData_VoxelToMm(int voxPos) {
	int ret = voxPos * VoxelSize - CubeSize / 2;
	return ret;
}

int voxelData_voxelPosToArrayIndex(int x, int y, int z) {
	int ret = x * VoxelsPerRow * VoxelsPerRow + y * VoxelsPerRow + z;
	if (ret >= VoxelsPerRow * VoxelsPerRow * VoxelsPerRow || ret < 0) {
		ret = 0;
	}
	return ret;
}


float voxelData_GetVoxelCertainty(int x, int y, int z) {
	if (VoxelRegion) {
		if (*voxelData_getVoxel(x, y, z) <= CERTAINTY_TRESHOLD_MIN) {
			return 0.0;
		} else if (*voxelData_getVoxel(x, y, z) < CERTAINTY_TRESHOLD_MAX) {
			return (float)(*voxelData_getVoxel(x, y, z)) / (float)CERTAINTY_TRESHOLD_MAX;
		} else {
			return 1.0f;
		}
	} else {
		return 0;
	}
}

int voxelData_GetVoxelState(int x, int y, int z) {
	float vc = voxelData_GetVoxelCertainty(x, y, z);
	if (vc == 0.0f) {
		return 0;
	} else if (vc == 1.0f) {
		return 2;
	} else {
		return 1;
	}
}

unsigned char * voxelData_getVoxel(int x, int y, int z) {
	if (VoxelRegion) {
		return &VoxelRegion[voxelData_voxelPosToArrayIndex(x, y, z)];
	} else {
		return 0;
	}
}

void voxelData_computePositiveVoxel(unsigned char * voxel) {
	if (*voxel <= CERTAINTY_TRESHOLD_MAX) {
		*voxel += CERTAINTY_SPEED_POSITIVE;
	}
}

void voxelData_computeNegativeVoxels() {
	if (VoxelRegion) {
		for (int i = 0; i < VoxelsPerRow * VoxelsPerRow * VoxelsPerRow; i++) {
			if (VoxelRegion[i] >= CERTAINTY_TRESHOLD_MIN && VoxelRegion[i] < CERTAINTY_TRESHOLD_MAX) {
				VoxelRegion[i] -= CERTAINTY_SPEED_NEGATIVE;
			}
		}
	}
}

unsigned char * voxelData_getVoxelFromMm(int mmX, int mmY, int mmZ) {
	if (VoxelRegion) {
		return &VoxelRegion[
			voxelData_voxelPosToArrayIndex(
				voxelData_mmToVoxel(mmX), 
				voxelData_mmToVoxel(mmY), 
				voxelData_mmToVoxel(mmZ)
			)
		];
	} else {
		return 0;
	}
}

void voxelData_Compute3dPoints(tFrameData * frame) {
	if (VoxelRegion) {
		for (int i = 0; i < 640 * 480; i++) {
			if (frame->p[i].x >= -CubeSize / 2 && frame->p[i].x <= CubeSize / 2 &&
				frame->p[i].y >= -CubeSize / 2 && frame->p[i].y <= CubeSize / 2 &&
				frame->p[i].z >= -CubeSize / 2 && frame->p[i].z <= CubeSize / 2)
			{
				voxelData_computePositiveVoxel(
					voxelData_getVoxelFromMm(
						frame->p[i].x,
						frame->p[i].y,
						frame->p[i].z
					)
				);
			}
		}
		voxelData_computeNegativeVoxels();
	}
}

int voxelData_GetVoxelsPerRow() {
	return VoxelsPerRow;
}

GRIDCELL voxelData_GetVoxelAsMarchingCubeGridcell(int x, int y, int z) {
	GRIDCELL ret;

	ret.p[0].x = voxelData_VoxelToMm(x);
	ret.p[0].y = voxelData_VoxelToMm(y);
	ret.p[0].z = voxelData_VoxelToMm(z);
	ret.val[0] = voxelData_GetVoxelCertainty(x, y, z);

	ret.p[1].x = voxelData_VoxelToMm(x + 1);
	ret.p[1].y = voxelData_VoxelToMm(y);
	ret.p[1].z = voxelData_VoxelToMm(z);
	ret.val[1] = voxelData_GetVoxelCertainty(x + 1, y, z);

	ret.p[2].x = voxelData_VoxelToMm(x + 1);
	ret.p[2].y = voxelData_VoxelToMm(y);
	ret.p[2].z = voxelData_VoxelToMm(z + 1);
	ret.val[2] = voxelData_GetVoxelCertainty(x + 1, y, z + 1);

	ret.p[3].x = voxelData_VoxelToMm(x);
	ret.p[3].y = voxelData_VoxelToMm(y);
	ret.p[3].z = voxelData_VoxelToMm(z + 1);
	ret.val[3] = voxelData_GetVoxelCertainty(x, y, z + 1);

	ret.p[4].x = voxelData_VoxelToMm(x);
	ret.p[4].y = voxelData_VoxelToMm(y + 1);
	ret.p[4].z = voxelData_VoxelToMm(z);
	ret.val[4] = voxelData_GetVoxelCertainty(x, y + 1, z);

	ret.p[5].x = voxelData_VoxelToMm(x + 1);
	ret.p[5].y = voxelData_VoxelToMm(y + 1);
	ret.p[5].z = voxelData_VoxelToMm(z);
	ret.val[5] = voxelData_GetVoxelCertainty(x + 1, y + 1, z);

	ret.p[6].x = voxelData_VoxelToMm(x + 1);
	ret.p[6].y = voxelData_VoxelToMm(y + 1);
	ret.p[6].z = voxelData_VoxelToMm(z + 1);
	ret.val[6] = voxelData_GetVoxelCertainty(x + 1, y + 1, z + 1);

	ret.p[7].x = voxelData_VoxelToMm(x);
	ret.p[7].y = voxelData_VoxelToMm(y + 1);
	ret.p[7].z = voxelData_VoxelToMm(z + 1);
	ret.val[7] = voxelData_GetVoxelCertainty(x, y + 1, z + 1);

	return ret;
}